#include "angle.h"
#include <math.h>

PID ang_pid;

void set_pid_by_param(Param* p)
{
	ang_pid.kp = p->list.ang_pid_p;
	ang_pid.ki = p->list.ang_pid_i;
	ang_pid.kd = p->list.ang_pid_d;
	ang_pid.imax = p->list.ang_pid_i_max;
	
	ang_pid.dt = 0.01; //10ms
	pid_reset(&ang_pid);
}

float get_angle_update_output(float target, float current)
{	
	pid_update(&ang_pid, target, current);   //0-100

	if(ang_pid.output>100.0f)
		ang_pid.output = 100.0f;
	else if(ang_pid.output<-100.0f)
		ang_pid.output = -100.0f;
			
	return ang_pid.output;
}

void angle_pid_reset(void)
{
	pid_reset(&ang_pid);
}
